/*
 * @Author: soon.liao
 * @Date: 2024-02-05 21:43:32
 * @LastEditors: soon.liao
 * @LastEditTime: 2024-02-07 07:32:39
 * @FilePath: \my-car\main\Mecanum.c
 * @Description: NA
 * NA
 * Copyright (c) 2024 by soon.liao  , All Rights Reserved.
 */
/*
 * SPDX-FileCopyrightText: 2010-2022 Espressif Systems (Shanghai) CO LTD
 *
 * SPDX-License-Identifier: CC0-1.0
 */
#include "esp_log.h"

#include "l9110s.h"
#include "Mecanum.h"

static const char *TAG = "Mecanum";

void mecanum_control_init(void)
{
    ESP_LOGI(TAG, "func=%s line=%d", __FUNCTION__, __LINE__);
    motor_l9110s_init();
}

void mecanum_control_direction(MecanumDirection direction)
{
    ESP_LOGI(TAG, "%s direction=%d", __FUNCTION__, direction);
    switch (direction)
    {
    case DIRE_FOREWARD:
        motor_l9110s_control(LEFT_FRONT, FOREWARD);
        motor_l9110s_control(LEFT_REAR, FOREWARD);
        motor_l9110s_control(RIGHT_FRONT, FOREWARD);
        motor_l9110s_control(RIGHT_REAR, FOREWARD);
        break;
    case DIRE_RETREAT:
        motor_l9110s_control(LEFT_FRONT, REVERSAL);
        motor_l9110s_control(LEFT_REAR, REVERSAL);
        motor_l9110s_control(RIGHT_FRONT, REVERSAL);
        motor_l9110s_control(RIGHT_REAR, REVERSAL);
        break;
    case DIRE_TRAN_RIGHT:
        motor_l9110s_control(LEFT_FRONT, FOREWARD);
        motor_l9110s_control(LEFT_REAR, REVERSAL);
        motor_l9110s_control(RIGHT_FRONT, REVERSAL);
        motor_l9110s_control(RIGHT_REAR, FOREWARD);
        break;
    case DIRE_TRAN_LEFT:
        motor_l9110s_control(LEFT_FRONT, REVERSAL);
        motor_l9110s_control(LEFT_REAR, FOREWARD);
        motor_l9110s_control(RIGHT_FRONT, FOREWARD);
        motor_l9110s_control(RIGHT_REAR, REVERSAL);
        break;
    case DIRE_DEXT_RIGHT:
        motor_l9110s_control(LEFT_FRONT, FOREWARD);
        motor_l9110s_control(LEFT_REAR, FOREWARD);
        motor_l9110s_control(RIGHT_FRONT, REVERSAL);
        motor_l9110s_control(RIGHT_REAR, REVERSAL);
        break;
    case DIRE_DEXT_LEFT:
        motor_l9110s_control(LEFT_FRONT, REVERSAL);
        motor_l9110s_control(LEFT_REAR, REVERSAL);
        motor_l9110s_control(RIGHT_FRONT, FOREWARD);
        motor_l9110s_control(RIGHT_REAR, FOREWARD);
        break;
    case DIRE_FRON_RIGHT:
        motor_l9110s_control(LEFT_FRONT, FOREWARD);
        motor_l9110s_control(LEFT_REAR, STANDBY);
        motor_l9110s_control(RIGHT_FRONT, STANDBY);
        motor_l9110s_control(RIGHT_REAR, FOREWARD);
        break;
    case DIRE_FRON_LEFT:
        motor_l9110s_control(LEFT_FRONT, STANDBY);
        motor_l9110s_control(LEFT_REAR, FOREWARD);
        motor_l9110s_control(RIGHT_FRONT, FOREWARD);
        motor_l9110s_control(RIGHT_REAR, STANDBY);
        break;
    case DIRE_REAR_RIGHT:
        motor_l9110s_control(LEFT_FRONT, STANDBY);
        motor_l9110s_control(LEFT_REAR, REVERSAL);
        motor_l9110s_control(RIGHT_FRONT, REVERSAL);
        motor_l9110s_control(RIGHT_REAR, STANDBY);
        break;
    case DIRE_REAR_LEFT:
        motor_l9110s_control(LEFT_FRONT, REVERSAL);
        motor_l9110s_control(LEFT_REAR, STANDBY);
        motor_l9110s_control(RIGHT_FRONT, STANDBY);
        motor_l9110s_control(RIGHT_REAR, REVERSAL);
        break;
    case DIRE_FRON_DEXT_RIGHT:
        motor_l9110s_control(LEFT_FRONT, STANDBY);
        motor_l9110s_control(LEFT_REAR, FOREWARD);
        motor_l9110s_control(RIGHT_FRONT, STANDBY);
        motor_l9110s_control(RIGHT_REAR, REVERSAL);
        break;
    case DIRE_FRON_DEXT_LEFT:
        motor_l9110s_control(LEFT_FRONT, STANDBY);
        motor_l9110s_control(LEFT_REAR, REVERSAL);
        motor_l9110s_control(RIGHT_FRONT, STANDBY);
        motor_l9110s_control(RIGHT_REAR, FOREWARD);
        break;
    case DIRE_BACK_DEXT_RIGHT:
        motor_l9110s_control(LEFT_FRONT, FOREWARD);
        motor_l9110s_control(LEFT_REAR, STANDBY);
        motor_l9110s_control(RIGHT_FRONT, REVERSAL);
        motor_l9110s_control(RIGHT_REAR, STANDBY);
        break;
    case DIRE_BACK_DEXT_LEFT:
        motor_l9110s_control(LEFT_FRONT, REVERSAL);
        motor_l9110s_control(LEFT_REAR, STANDBY);
        motor_l9110s_control(RIGHT_FRONT, FOREWARD);
        motor_l9110s_control(RIGHT_REAR, STANDBY);
        break;
    case DIRE_BRAKE:
        motor_l9110s_control(LEFT_FRONT, DIRE_BRAKE);
        motor_l9110s_control(LEFT_REAR, DIRE_BRAKE);
        motor_l9110s_control(RIGHT_FRONT, DIRE_BRAKE);
        motor_l9110s_control(RIGHT_REAR, DIRE_BRAKE);
        break;
    case DIRE_STANDBY:
        motor_l9110s_control(LEFT_FRONT, STANDBY);
        motor_l9110s_control(LEFT_REAR, STANDBY);
        motor_l9110s_control(RIGHT_FRONT, STANDBY);
        motor_l9110s_control(RIGHT_REAR, STANDBY);
        break;

    default:
        break;
    }
}
